#include "V4L2Camera.hpp"
#include "cbb_camera_api.h"
#include <chrono>
#include <cstring>
#include <errno.h>
#include <fcntl.h>
#include <iostream>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <thread>
#include <unistd.h>

V4L2Camera::V4L2Camera(const V4L2CameraConfig &config)
    : m_config(config) {}

V4L2Camera::~V4L2Camera() {
    close();
}

bool V4L2Camera::open() {
    m_handle = CbbCameraOpen(m_config.deviceName.c_str());
    if (m_handle == nullptr) {
        return false;
    }
    m_isOpen = true;
    setConfig();
    return true;
}

bool V4L2Camera::close() {
    if (!m_isOpen) {
        return true;
    }

    CbbCameraAckE_t result = CbbCameraClose(m_handle);
    m_isOpen = false;
    m_handle = nullptr;
    return (result == CBB_CAMERA_ACK_OK);
}

bool V4L2Camera::startCapture() {
    if (!m_isOpen) {
        return false;
    }
    CbbCameraCtrlDev_t camera_ctrl_cap;
    camera_ctrl_cap.camera_ctrl_u.action = CBB_CAMERA_START;

    CbbCameraAckE_t result = CbbCameraCtrl(m_handle, CBB_CAMERA_ACT_CTL_CAP, &camera_ctrl_cap);
    return (result == CBB_CAMERA_ACK_OK);
}

bool V4L2Camera::stopCapture() {
    if (!m_isOpen) {
        return false;
    }
    CbbCameraCtrlDev_t camera_ctrl_cap;
    camera_ctrl_cap.camera_ctrl_u.action = CBB_CAMERA_STOP;
    CbbCameraAckE_t result = CbbCameraCtrl(m_handle, CBB_CAMERA_ACT_CTL_CAP, &camera_ctrl_cap);
    return (result == CBB_CAMERA_ACK_OK);
}

bool V4L2Camera::captureFrame(CbbCameraFrame_t &frame, int timeout_ms) {
    if (!m_isOpen) {
        return false;
    }
    CbbCameraAckE_t result = CbbCameraReadFrame(m_handle, &frame, timeout_ms);
    return (result == CBB_CAMERA_ACK_OK);
}

bool V4L2Camera::releaseFrame(CbbCameraFrame_t &frame) {
    if (!m_isOpen) {
        return false;
    }
    CbbCameraAckE_t result = CbbCameraRestFrame(m_handle, frame);
    return (result == CBB_CAMERA_ACK_OK);
}

bool V4L2Camera::updateConfig(const V4L2CameraConfig &config) {
    m_config = config;
    if (m_isOpen) {
        return setConfig();
    }
    return true;
}

bool V4L2Camera::setConfig() {
    if (!m_isOpen || m_handle == nullptr) {
        return false;
    }

    CbbCameraCtrlDev_t ctrlDev;
    ctrlDev.camera_ctrl_u.conf_s.width = m_config.width;
    ctrlDev.camera_ctrl_u.conf_s.height = m_config.height;
    ctrlDev.camera_ctrl_u.conf_s.fps = m_config.fps;
    ctrlDev.camera_ctrl_u.conf_s.format = m_config.format;

    std::cout << "Setting camera configuration: " << m_config.deviceName << " " << m_config.width << "x" << m_config.height << "@" << m_config.fps << "fps, format: " << m_config.format << std::endl;
    CbbCameraAckE_t result = CbbCameraCtrl(m_handle, CBB_CAMERA_ACT_SET_CONF, &ctrlDev);
    return (result == CBB_CAMERA_ACK_OK);
}